#ifndef __SG_PCL_CONVERSION_H__
#define __SG_PCL_CONVERSION_H__

#include "SGPCLBaseInfo.h"
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

namespace sgpcl
{

/**
 * @brief 从norm点云转为XYZ点云
 * @param spNorm
 * @return
 */
template <typename PointOut>
typename pcl::PointCloud<PointOut>::Ptr NormToXYZ(pcl::PointCloud<pcl::Normal>::ConstPtr spNorm)
{
  pcl::PointCloud<PointOut>::Ptr spXYZCloud(new pcl::PointCloud<PointOut>);

  const int nPointNum = spNorm->points.size();
  spXYZCloud->resize(nPointNum);

  // 所有向量落在z>0的四个象限中
#pragma omp parallel for
  for (int nPointID = 0; nPointID < nPointNum; ++nPointID)
  {
    const int fDir = (spNorm->at(nPointID).normal_z > 0) ? 1 : -1;
    spXYZCloud->at(nPointID).x = spNorm->at(nPointID).normal_x * fDir;
    spXYZCloud->at(nPointID).y = spNorm->at(nPointID).normal_y * fDir;
    spXYZCloud->at(nPointID).z = spNorm->at(nPointID).normal_z * fDir;
  }

  return spXYZCloud;
}

/**
 * @brief 将norm点云赋值给带Normal属性的点云
 * @param spNorm norm点云
 * @param spPointNorm 带Normal属性的点云
 * @return 修改后的带Normal属性的点云智能指针
 */
template <typename PointIn, typename PointOut>
typename pcl::PointCloud<PointOut>::Ptr AddNormToPoint(
  typename pcl::PointCloud<PointIn>::ConstPtr spNorm,
  typename pcl::PointCloud<PointOut>::Ptr spPointNorm)
{
  const int nPointNum = spNorm->points.size();
  const int nOutPointNum = spPointNorm->points.size();
  if (nPointNum != nOutPointNum)
  {
    SG_PANIC("无法合并维度不一样的点云！");
  }

#pragma omp parallel for
  for (int nPointID = 0; nPointID < nPointNum; ++nPointID)
  {
    const int fDir = (spNorm->at(nPointID).normal_z > 0) ? 1 : -1; // 所有向量落在z>0的四个象限中
    spPointNorm->at(nPointID).normal_x = spNorm->at(nPointID).normal_x * fDir;
    spPointNorm->at(nPointID).normal_y = spNorm->at(nPointID).normal_y * fDir;
    spPointNorm->at(nPointID).normal_z = spNorm->at(nPointID).normal_z * fDir;
  }
  return spPointNorm;
}

/**
 * @brief 将curve曲率点云赋值给带曲率属性的点云
 * @param spNorm norm点云
 * @param spPointNorm 带曲率属性的点云
 * @return 修改后的带曲率属性的点云智能指针
 */
template <typename PointIn, typename PointOut>
typename pcl::PointCloud<PointOut>::Ptr AddCurveToPoint(
  typename pcl::PointCloud<PointIn>::ConstPtr spCurve,
  typename pcl::PointCloud<PointOut>::Ptr spPointNorm)
{
  const int nPointNum = spCurve->points.size();
  const int nOutPointNum = spPointNorm->points.size();
  if (nPointNum != nOutPointNum)
  {
    SG_PANIC("无法合并维度不一样的点云！");
  }

#pragma omp parallel for
  for (int nPointID = 0; nPointID < nPointNum; ++nPointID)
  {
    spPointNorm->at(nPointID).curvature = spCurve->at(nPointID).pc1; // 经实验，最大曲率效果较好
  }
  return spPointNorm;
}

}

#endif // !__SG_PCL_CONVERSION_H__
